#include <cassert>
#include <algorithm>

#include "../../../Exceptions.h"

#include "KinematicSensor.h"

using namespace MMM;

KinematicSensorPtr KinematicSensor::loadSensorXML(RapidXMLReaderNodePtr node) {
    KinematicSensorPtr sensor(new KinematicSensor());
    sensor->loadSensor(node);
    return sensor;
}

KinematicSensor::KinematicSensor() : InterpolatableSensor()
{
}

KinematicSensor::KinematicSensor(const std::vector<std::string> &jointNames, const std::string &description) :
    InterpolatableSensor(description),
    jointNames(jointNames)
{
}

bool KinematicSensor::checkModel(ModelPtr model) {
    if (!model) {
        MMM_ERROR << "Sensor '" << uniqueName << "' needs a model."  << std::endl;
        return false;
    }
    for (auto jointName : jointNames) {
        if (!model->getModelNode(jointName)) return false;
    }
    return true;
}

boost::shared_ptr<BasicSensor<KinematicSensorMeasurement> > KinematicSensor::cloneConfiguration() {
    KinematicSensorPtr k(new KinematicSensor(jointNames, description));
    return k;
}

void KinematicSensor::loadConfigurationXML(RapidXMLReaderNodePtr node) {
    assert(node);
    assert(node->name() == "Configuration");

    for(auto jointNode : node->nodes("Joint")) {
        std::string jointName = jointNode->attribute_value(XML::ATTRIBUTE_NAME);
        if (std::find(jointNames.begin(), jointNames.end(), jointName) == jointNames.end()) jointNames.push_back(jointName);
        else throw Exception::XMLFormatException("Duplicate of joint '" + jointName + "' in Kinematics sensor configuration");
    }
}

void KinematicSensor::loadMeasurementXML(RapidXMLReaderNodePtr node, float timestep, SensorMeasurementType type)
{
    assert(node);
    assert(node->name() == XML::NODE_MEASUREMENT);

    KinematicSensorMeasurementPtr k(new KinematicSensorMeasurement(timestep, XML::readFloatArray(node->first_node("JointPosition")->value(), jointNames.size()), type));
    addSensorMeasurement(k); //never false, because the error is catched shortly before
}

void KinematicSensor::appendConfigurationXML(RapidXMLWriterNodePtr node)
{
    assert(node);

    RapidXMLWriterNodePtr configurationNode = node->append_node("Configuration");
    for(auto joint : jointNames) {
        configurationNode->append_node("Joint")->append_attribute(XML::ATTRIBUTE_NAME, joint);
    }
}

bool KinematicSensor::hasSensorConfigurationContent() const
{
    return !(this->jointNames.empty());
}

bool KinematicSensor::equalsConfiguration(SensorPtr other) {
    KinematicSensorPtr ptr = boost::dynamic_pointer_cast<KinematicSensor>(other);
    if (ptr) {

        if (jointNames == ptr->jointNames) return true;
        else {
            for (auto jointName : ptr->jointNames) {
                if (std::find(jointNames.begin(), jointNames.end(), jointName) == jointNames.end()) return false; // TODO
            }
            return false;
        }
    }
    else return false;
}

bool KinematicSensor::addSensorMeasurement(KinematicSensorMeasurementPtr measurement)
{
    if (static_cast<std::size_t>(measurement->getJointAngles().rows()) != jointNames.size()) {
        MMM_ERROR << "Could not add kinematic sensor measurement, because the jointAngles are not matching the jointNames" << std::endl;
        return false;
    }
    measurements.insert(std::pair<float, KinematicSensorMeasurementPtr>(measurement->getTimestep(), measurement));
    return true;
}

std::vector<std::string> KinematicSensor::getJointNames() {
    return jointNames;
}

std::string KinematicSensor::getType() {
    return TYPE;
}

std::string KinematicSensor::getVersion() {
    return VERSION;
}

int KinematicSensor::getPriority() {
    return 90;
}

KinematicSensorPtr KinematicSensor::join(std::vector<KinematicSensorPtr> sensors) {
    std::vector<float> timesteps;
    int max_jSize = 0;
    for (auto sensor : sensors) {
        int jSize = sensor->getJointNames().size();
        if (jSize > max_jSize) {
            max_jSize = jSize;
            timesteps = sensor->getTimesteps();
        }
    }
    return join(sensors, timesteps);
}

KinematicSensorPtr KinematicSensor::join(std::vector<KinematicSensorPtr> sensors, const std::vector<float> &timesteps) {
    if (sensors.size() == 0) throw Exception::MMMException("Could not join kinematic sensors, because there are none!");

    std::vector<std::string> jointNames;
    for (auto sensor : sensors) {
        for (auto jointName : sensor->getJointNames()) {
            if (std::find(jointNames.begin(), jointNames.end(), jointName) != jointNames.end())
                throw Exception::MMMException("Could not join kinematic sensors, because the joint names of different kinematic sensors are the same.");
            jointNames.push_back(jointName);
        }
    }
    KinematicSensorPtr joinedSensor(new KinematicSensor(jointNames, "Joined sensor."));

    for (auto timestep : timesteps) {
        Eigen::VectorXf jointAngles(jointNames.size());
        SensorMeasurementType type = SensorMeasurementType::MEASURED;
        int j = 0;
        for (auto sensor : sensors) {
            KinematicSensorMeasurementPtr sensorMeasurement = sensor->getDerivedMeasurement(timestep);
            // set not interpolatable sensor measurements zero. maybe TODO other method!
            if (!sensorMeasurement) sensorMeasurement = KinematicSensorMeasurementPtr(new KinematicSensorMeasurement(timestep, Eigen::VectorXf::Zero(sensor->getJointNames().size(), true)));
            if (type != SensorMeasurementType::EXTENDED) {
                if (sensorMeasurement->isExtended()) type = SensorMeasurementType::EXTENDED;
                else if (sensorMeasurement->isInterpolated()) type = SensorMeasurementType::INTERPOLATED;
            }
            Eigen::VectorXf sensorMeasurement_jointAngles = sensorMeasurement->getJointAngles();
            for (unsigned int i = 0; i < sensorMeasurement_jointAngles.rows(); i++, j++)
                jointAngles(j) = sensorMeasurement_jointAngles(i);
        }
        KinematicSensorMeasurementPtr joinedSensorMeasurement(new KinematicSensorMeasurement(timestep, jointAngles, type));
        joinedSensor->addSensorMeasurement(joinedSensorMeasurement);
    }

    return joinedSensor;
}
